#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/NavSatFix.h>
#include <std_msgs/Float32.h>
#include <tf2/LinearMath/Transform.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Vector3.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/registration/ndt.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/io/pcd_io.h>
#include <string>
#include <fstream>
#include <vector>
#include <cmath>
#include <mutex>
#include <Eigen/Dense>

class NDTLocalizer
{
private:
    // ROS节点句柄
    ros::NodeHandle nh_;
    ros::NodeHandle private_nh_;

    // 订阅者和发布者
    ros::Subscriber points_sub_;
    ros::Subscriber imu_sub_;
    ros::Subscriber odom_sub_;
    ros::Subscriber gnss_sub_;
    ros::Publisher current_pose_pub_;
    ros::Publisher current_scan_pub_;
    ros::Publisher ndt_pose_pub_;
    ros::Publisher ndt_stat_pub_;
    ros::Publisher twist_pub_;

    // TF广播器和监听器
    tf2_ros::TransformBroadcaster tf_broadcaster_;
    tf2_ros::Buffer tf_buffer_;
    tf2_ros::TransformListener tf_listener_;

    // 点云和位姿
    pcl::PointCloud<pcl::PointXYZ>::Ptr map_cloud_;
    pcl::PointCloud<pcl::PointXYZ>::Ptr current_cloud_;
    tf2::Transform current_pose_;
    tf2::Transform previous_pose_;
    tf2::Transform initial_pose_;
    tf2::Transform imu_pose_;
    tf2::Transform odom_pose_;
    tf2::Transform gnss_pose_;
    
    // 位姿估计来源
    enum PoseEstimationSource {
        INIT_POSE,
        PREDICT,
        IMU,
        ODOM,
        GNSS
    };
    PoseEstimationSource pose_estimation_source_;
    
    // 互斥锁
    std::mutex pose_mutex_;
    std::mutex map_mutex_;
    
    // 标志位
    bool is_map_loaded_;
    bool is_initialized_;
    bool use_imu_;
    bool use_odom_;
    bool use_gnss_;
    bool use_predict_;
    bool use_gnss_for_init_;
    
    // 时间戳
    ros::Time prev_scan_time_;
    
    // 速度和加速度
    double current_velocity_;
    double current_angular_velocity_;
    double current_acceleration_;
    
    // 定位参数
    double voxel_leaf_size_;
    double ndt_resolution_;
    double ndt_step_size_;
    double ndt_epsilon_;
    int max_iterations_;
    double outlier_ratio_;
    
    // 重定位参数
    double gnss_reinit_threshold_;
    int consecutive_failure_threshold_;
    int consecutive_failures_;
    
    // 坐标系参数
    std::string map_frame_id_;
    std::string base_frame_id_;
    std::string lidar_frame_id_;
    
    // 地图文件路径
    std::string map_file_path_;
    
    // 上一次NDT得分
    double last_ndt_score_;
    
    // 位姿协方差
    Eigen::Matrix<double, 6, 6> pose_covariance_;

    // 初始GNSS数据
    double initial_latitude_;
    double initial_longitude_;
    double initial_altitude_;
    bool has_initial_gnss_;

public:
    NDTLocalizer() : private_nh_("~"), tf_listener_(tf_buffer_), 
                     is_map_loaded_(false), is_initialized_(false),
                     current_velocity_(0.0), current_angular_velocity_(0.0),
                     current_acceleration_(0.0), consecutive_failures_(0),
                     initial_latitude_(0.0), initial_longitude_(0.0), 
                     initial_altitude_(0.0), has_initial_gnss_(false)
    {
        // 初始化点云
        map_cloud_.reset(new pcl::PointCloud<pcl::PointXYZ>());
        current_cloud_.reset(new pcl::PointCloud<pcl::PointXYZ>());
        
        // 初始化位姿
        current_pose_.setIdentity();
        previous_pose_.setIdentity();
        initial_pose_.setIdentity();
        imu_pose_.setIdentity();
        odom_pose_.setIdentity();
        gnss_pose_.setIdentity();
        
        // 获取参数
        private_nh_.param<double>("voxel_leaf_size", voxel_leaf_size_, 0.2);
        private_nh_.param<double>("ndt_resolution", ndt_resolution_, 1.0);
        private_nh_.param<double>("ndt_step_size", ndt_step_size_, 0.1);
        private_nh_.param<double>("ndt_epsilon", ndt_epsilon_, 0.01);
        private_nh_.param<int>("max_iterations", max_iterations_, 30);
        private_nh_.param<double>("outlier_ratio", outlier_ratio_, 0.55);
        
        private_nh_.param<bool>("use_imu", use_imu_, false);
        private_nh_.param<bool>("use_odom", use_odom_, false);
        private_nh_.param<bool>("use_gnss", use_gnss_, false);
        private_nh_.param<bool>("use_predict", use_predict_, true);
        private_nh_.param<bool>("use_gnss_for_init", use_gnss_for_init_, true);
        
        private_nh_.param<double>("gnss_reinit_threshold", gnss_reinit_threshold_, 5.0);
        private_nh_.param<int>("consecutive_failure_threshold", consecutive_failure_threshold_, 5);
        
        private_nh_.param<std::string>("map_frame_id", map_frame_id_, "map");
        private_nh_.param<std::string>("base_frame_id", base_frame_id_, "base_link");
        private_nh_.param<std::string>("lidar_frame_id", lidar_frame_id_, "velodyne");
        
        // 使用硬编码的地图文件路径
        map_file_path_ = "/root/catkin_ws/src/auto_navigation/maps/20250317_155459/kitti_map_20250317_160805.pcd";
        
        // 打印参数
        ROS_INFO("NDT Localizer parameters:");
        ROS_INFO("voxel_leaf_size: %.3f", voxel_leaf_size_);
        ROS_INFO("ndt_resolution: %.3f", ndt_resolution_);
        ROS_INFO("ndt_step_size: %.3f", ndt_step_size_);
        ROS_INFO("ndt_epsilon: %.5f", ndt_epsilon_);
        ROS_INFO("max_iterations: %d", max_iterations_);
        ROS_INFO("outlier_ratio: %.3f", outlier_ratio_);
        ROS_INFO("map_file_path: %s", map_file_path_.c_str());
        
        // 检查地图文件路径
        if (map_file_path_.empty())
        {
            ROS_ERROR("Map file path is empty");
            return;
        }
        
        // 加载地图
        if (!loadMap(map_file_path_))
        {
            ROS_ERROR("Failed to load map from %s", map_file_path_.c_str());
            return;
        }
        
        // 订阅点云话题
        points_sub_ = nh_.subscribe("velodyne_points", 10, &NDTLocalizer::pointsCallback, this);
        
        // 根据配置订阅IMU、里程计和GNSS话题
        if (use_imu_)
        {
            imu_sub_ = nh_.subscribe("imu/data", 10, &NDTLocalizer::imuCallback, this);
            ROS_INFO("Using IMU for pose estimation");
        }
        
        if (use_odom_)
        {
            odom_sub_ = nh_.subscribe("odom", 10, &NDTLocalizer::odomCallback, this);
            ROS_INFO("Using Odometry for pose estimation");
        }
        
        if (use_gnss_)
        {
            gnss_sub_ = nh_.subscribe("gnss/pose", 10, &NDTLocalizer::gnssCallback, this);
            ROS_INFO("Using GNSS for pose estimation and reinitialization");
        }
        
        // 发布位姿和点云
        current_pose_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("ndt/current_pose", 10);
        current_scan_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("ndt/current_scan", 10);
        ndt_pose_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("ndt/pose_with_covariance", 10);
        ndt_stat_pub_ = nh_.advertise<std_msgs::Float32>("ndt/score", 10);
        twist_pub_ = nh_.advertise<geometry_msgs::TwistStamped>("ndt/twist", 10);
        
        pose_estimation_source_ = INIT_POSE;
        
        ROS_INFO("NDT Localizer initialized");
        ROS_INFO("Map loaded from: %s", map_file_path_.c_str());
        ROS_INFO("Map size: %lu points", map_cloud_->size());
    }
    
    ~NDTLocalizer()
    {
        ROS_INFO("NDT Localizer shutting down");
    }
    
    bool loadMap(const std::string& map_file)
    {
        if (pcl::io::loadPCDFile<pcl::PointXYZ>(map_file, *map_cloud_) == -1)
        {
            ROS_ERROR("Failed to load map from %s", map_file.c_str());
            return false;
        }
        
        ROS_INFO("Map loaded successfully from %s", map_file.c_str());
        ROS_INFO("Map size: %lu points", map_cloud_->size());
        
        is_map_loaded_ = true;
        return true;
    }
    
    void imuCallback(const sensor_msgs::Imu::ConstPtr& msg)
    {
        if (!is_initialized_)
            return;
            
        std::lock_guard<std::mutex> lock(pose_mutex_);
        
        // 提取IMU四元数
        tf2::Quaternion imu_quat;
        tf2::fromMsg(msg->orientation, imu_quat);
        
        // 更新IMU位姿（只更新旋转部分）
        imu_pose_.setRotation(imu_quat);
        
        // 提取角速度和线性加速度
        current_angular_velocity_ = std::sqrt(
            std::pow(msg->angular_velocity.x, 2) +
            std::pow(msg->angular_velocity.y, 2) +
            std::pow(msg->angular_velocity.z, 2));
            
        current_acceleration_ = std::sqrt(
            std::pow(msg->linear_acceleration.x, 2) +
            std::pow(msg->linear_acceleration.y, 2) +
            std::pow(msg->linear_acceleration.z, 2));
    }
    
    void odomCallback(const nav_msgs::Odometry::ConstPtr& msg)
    {
        if (!is_initialized_)
            return;
            
        std::lock_guard<std::mutex> lock(pose_mutex_);
        
        // 提取里程计位姿
        tf2::Transform odom_transform;
        tf2::fromMsg(msg->pose.pose, odom_transform);
        
        // 更新里程计位姿
        odom_pose_ = odom_transform;
        
        // 提取线速度
        current_velocity_ = std::sqrt(
            std::pow(msg->twist.twist.linear.x, 2) +
            std::pow(msg->twist.twist.linear.y, 2) +
            std::pow(msg->twist.twist.linear.z, 2));
    }
    
    void gnssCallback(const sensor_msgs::NavSatFix::ConstPtr& msg)
    {
        // 保存第一个GNSS数据点作为参考
        if (!has_initial_gnss_)
        {
            initial_latitude_ = msg->latitude;
            initial_longitude_ = msg->longitude;
            initial_altitude_ = msg->altitude;
            has_initial_gnss_ = true;
            ROS_INFO("Initial GNSS position: lat=%.6f, lon=%.6f, alt=%.2f", 
                     initial_latitude_, initial_longitude_, initial_altitude_);
        }
        
        if (!is_initialized_ && use_gnss_for_init_ && has_initial_gnss_)
        {
            // 使用GNSS数据初始化位姿
            // 创建一个简单的变换，将GNSS位置设为地图原点附近
            tf2::Vector3 position(0.0, 0.0, 0.0);  // 初始位置设为原点
            tf2::Quaternion orientation;
            orientation.setRPY(0.0, 0.0, 0.0);  // 初始方向设为零
            
            initial_pose_.setOrigin(position);
            initial_pose_.setRotation(orientation);
            
            current_pose_ = initial_pose_;
            previous_pose_ = initial_pose_;
            
            is_initialized_ = true;
            pose_estimation_source_ = GNSS;
            
            ROS_INFO("Initialized with GNSS data");
            return;
        }
        
        if (!is_initialized_ || !has_initial_gnss_)
            return;
        
        std::lock_guard<std::mutex> lock(pose_mutex_);
        
        // 将GNSS数据转换为位姿
        // 计算相对于初始位置的偏移
        double lat_diff = msg->latitude - initial_latitude_;
        double lon_diff = msg->longitude - initial_longitude_;
        
        // 简单的经纬度到米的转换
        double meters_per_lat = 111320.0;  // 每度纬度约111320米
        double meters_per_lon = 111320.0 * cos(msg->latitude * M_PI / 180.0);  // 每度经度的米数取决于纬度
        
        double x = lon_diff * meters_per_lon;
        double y = lat_diff * meters_per_lat;
        double z = msg->altitude - initial_altitude_;
        
        tf2::Vector3 position(x, y, z);
        
        // 保持当前的方向
        tf2::Quaternion orientation = current_pose_.getRotation();
        
        gnss_pose_.setOrigin(position);
        gnss_pose_.setRotation(orientation);
        
        // 检查是否需要重新初始化
        tf2::Vector3 current_position = current_pose_.getOrigin();
        tf2::Vector3 gnss_position = gnss_pose_.getOrigin();
        
        double distance = sqrt(pow(current_position.x() - gnss_position.x(), 2) +
                              pow(current_position.y() - gnss_position.y(), 2) +
                              pow(current_position.z() - gnss_position.z(), 2));
        
        if (distance > gnss_reinit_threshold_)
        {
            ROS_WARN("Large position difference detected (%.2f m), reinitializing with GNSS", distance);
            current_pose_ = gnss_pose_;
            previous_pose_ = gnss_pose_;
            pose_estimation_source_ = GNSS;
        }
    }
    
    void pointsCallback(const sensor_msgs::PointCloud2ConstPtr& msg)
    {
        if (!is_map_loaded_)
        {
            ROS_WARN_THROTTLE(5, "Map not loaded yet, skipping scan");
            return;
        }
        
        // 将ROS消息转换为PCL点云
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
        pcl::fromROSMsg(*msg, *cloud);
        
        // 体素滤波降采样
        pcl::VoxelGrid<pcl::PointXYZ> voxel_filter;
        voxel_filter.setInputCloud(cloud);
        voxel_filter.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, voxel_leaf_size_);
        voxel_filter.filter(*current_cloud_);
        
        // 如果点云为空，跳过
        if (current_cloud_->empty())
        {
            ROS_WARN_THROTTLE(5, "Empty point cloud after filtering, skipping");
            return;
        }
        
        // 如果尚未初始化且不使用GNSS初始化，使用原点初始化
        if (!is_initialized_ && !use_gnss_for_init_)
        {
            current_pose_.setIdentity();
            initial_pose_.setIdentity();
            previous_pose_.setIdentity();
            is_initialized_ = true;
            pose_estimation_source_ = INIT_POSE;
            ROS_INFO("Initialized pose at origin");
        }
        
        // 如果已初始化，执行NDT定位
        if (is_initialized_)
        {
            // 获取初始位姿估计
            tf2::Transform init_guess = getPoseEstimation();
            
            // 执行NDT配准
            performNDT(init_guess, msg->header.stamp);
            
            // 发布位姿和点云
            publishPoseAndScan(msg->header.stamp);
            
            // 更新上一帧位姿
            previous_pose_ = current_pose_;
            prev_scan_time_ = msg->header.stamp;
        }
        else
        {
            ROS_WARN_THROTTLE(5, "Not initialized yet, waiting for initialization");
        }
    }
    
    tf2::Transform getPoseEstimation()
    {
        std::lock_guard<std::mutex> lock(pose_mutex_);
        
        tf2::Transform init_guess;
        
        // 根据配置选择位姿估计来源
        if (use_predict_ && prev_scan_time_.toSec() > 0)
        {
            // 使用速度预测位姿
            double dt = (ros::Time::now() - prev_scan_time_).toSec();
            
            // 简单的运动模型：位置 = 上一位置 + 速度 * 时间
            tf2::Vector3 displacement(current_velocity_ * dt, 0, 0);
            displacement = previous_pose_.getBasis() * displacement;
            
            init_guess = previous_pose_;
            init_guess.setOrigin(previous_pose_.getOrigin() + displacement);
            
            pose_estimation_source_ = PREDICT;
        }
        else if (use_imu_)
        {
            // 使用IMU位姿（只使用旋转部分）
            init_guess = previous_pose_;
            init_guess.setRotation(imu_pose_.getRotation());
            
            pose_estimation_source_ = IMU;
        }
        else if (use_odom_)
        {
            // 使用里程计位姿
            init_guess = odom_pose_;
            
            pose_estimation_source_ = ODOM;
        }
        else
        {
            // 使用上一帧位姿
            init_guess = previous_pose_;
            
            pose_estimation_source_ = INIT_POSE;
        }
        
        return init_guess;
    }
    
    void performNDT(const tf2::Transform& init_guess, const ros::Time& timestamp)
    {
        std::lock_guard<std::mutex> lock(map_mutex_);
        
        // 使用NDT算法进行配准
        pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
        ndt.setTransformationEpsilon(ndt_epsilon_);
        ndt.setStepSize(ndt_step_size_);
        ndt.setResolution(ndt_resolution_);
        ndt.setMaximumIterations(max_iterations_);
        ndt.setOulierRatio(outlier_ratio_);
        
        ndt.setInputSource(current_cloud_);
        ndt.setInputTarget(map_cloud_);
        
        // 使用初始位姿估计作为初始猜测
        Eigen::Matrix4f init_guess_matrix = Eigen::Matrix4f::Identity();
        tf2::Matrix3x3 rot_mat(init_guess.getRotation());
        Eigen::Matrix3f eigen_rot;
        eigen_rot << rot_mat[0][0], rot_mat[0][1], rot_mat[0][2],
                     rot_mat[1][0], rot_mat[1][1], rot_mat[1][2],
                     rot_mat[2][0], rot_mat[2][1], rot_mat[2][2];
        init_guess_matrix.block<3, 3>(0, 0) = eigen_rot;
        init_guess_matrix(0, 3) = init_guess.getOrigin().x();
        init_guess_matrix(1, 3) = init_guess.getOrigin().y();
        init_guess_matrix(2, 3) = init_guess.getOrigin().z();
        
        // 执行配准
        pcl::PointCloud<pcl::PointXYZ>::Ptr aligned_cloud(new pcl::PointCloud<pcl::PointXYZ>());
        ndt.align(*aligned_cloud, init_guess_matrix);
        
        // 获取NDT得分
        double ndt_score = ndt.getFitnessScore();
        last_ndt_score_ = ndt_score;
        
        // 发布NDT得分
        std_msgs::Float32 score_msg;
        score_msg.data = ndt_score;
        ndt_stat_pub_.publish(score_msg);
        
        // 输出NDT得分和位姿估计来源
        std::string source_str;
        switch (pose_estimation_source_)
        {
            case INIT_POSE: source_str = "INIT"; break;
            case PREDICT: source_str = "PREDICT"; break;
            case IMU: source_str = "IMU"; break;
            case ODOM: source_str = "ODOM"; break;
            case GNSS: source_str = "GNSS"; break;
            default: source_str = "UNKNOWN"; break;
        }
        ROS_INFO("NDT score: %.3f, Pose source: %s", ndt_score, source_str.c_str());
        
        // 如果配准成功，更新位姿
        if (ndt.hasConverged())
        {
            // 获取变换矩阵
            Eigen::Matrix4f transform = ndt.getFinalTransformation();
            
            // 更新当前位姿
            tf2::Matrix3x3 tf_rot;
            tf_rot.setValue(
                transform(0, 0), transform(0, 1), transform(0, 2),
                transform(1, 0), transform(1, 1), transform(1, 2),
                transform(2, 0), transform(2, 1), transform(2, 2)
            );
            tf2::Quaternion tf_quat;
            tf_rot.getRotation(tf_quat);
            current_pose_.setOrigin(tf2::Vector3(transform(0, 3), transform(1, 3), transform(2, 3)));
            current_pose_.setRotation(tf_quat);
            
            // 设置默认协方差矩阵
            pose_covariance_ = Eigen::Matrix<double, 6, 6>::Identity() * 0.01;
            
            // 重置连续失败计数
            consecutive_failures_ = 0;
        }
        else
        {
            ROS_WARN("NDT did not converge!");
            consecutive_failures_++;
        }
    }
    
    void publishPoseAndScan(const ros::Time& timestamp)
    {
        // 发布当前位姿
        geometry_msgs::PoseStamped pose_msg;
        pose_msg.header.stamp = timestamp;
        pose_msg.header.frame_id = map_frame_id_;
        pose_msg.pose.position.x = current_pose_.getOrigin().x();
        pose_msg.pose.position.y = current_pose_.getOrigin().y();
        pose_msg.pose.position.z = current_pose_.getOrigin().z();
        pose_msg.pose.orientation.x = current_pose_.getRotation().x();
        pose_msg.pose.orientation.y = current_pose_.getRotation().y();
        pose_msg.pose.orientation.z = current_pose_.getRotation().z();
        pose_msg.pose.orientation.w = current_pose_.getRotation().w();
        current_pose_pub_.publish(pose_msg);
        
        // 发布带协方差的位姿
        geometry_msgs::PoseWithCovarianceStamped pose_cov_msg;
        pose_cov_msg.header = pose_msg.header;
        pose_cov_msg.pose.pose = pose_msg.pose;
        
        // 填充协方差矩阵
        for (int i = 0; i < 6; i++)
        {
            for (int j = 0; j < 6; j++)
            {
                pose_cov_msg.pose.covariance[i * 6 + j] = pose_covariance_(i, j);
            }
        }
        ndt_pose_pub_.publish(pose_cov_msg);
        
        // 发布当前点云
        sensor_msgs::PointCloud2 scan_msg;
        pcl::toROSMsg(*current_cloud_, scan_msg);
        scan_msg.header.stamp = timestamp;
        scan_msg.header.frame_id = map_frame_id_;
        current_scan_pub_.publish(scan_msg);
        
        // 计算并发布速度
        if (prev_scan_time_.toSec() > 0)
        {
            double dt = (timestamp - prev_scan_time_).toSec();
            if (dt > 0)
            {
                // 计算位移和旋转变化
                tf2::Vector3 pos_diff = current_pose_.getOrigin() - previous_pose_.getOrigin();
                tf2::Quaternion q1 = previous_pose_.getRotation();
                tf2::Quaternion q2 = current_pose_.getRotation();
                tf2::Quaternion q_diff = q2 * q1.inverse();
                tf2::Vector3 rpy_diff;
                tf2::Matrix3x3(q_diff).getRPY(rpy_diff[0], rpy_diff[1], rpy_diff[2]);
                
                // 计算线速度和角速度
                double vx = pos_diff.x() / dt;
                double vy = pos_diff.y() / dt;
                double vz = pos_diff.z() / dt;
                double wx = rpy_diff.x() / dt;
                double wy = rpy_diff.y() / dt;
                double wz = rpy_diff.z() / dt;
                
                // 发布速度消息
                geometry_msgs::TwistStamped twist_msg;
                twist_msg.header.stamp = timestamp;
                twist_msg.header.frame_id = base_frame_id_;
                twist_msg.twist.linear.x = vx;
                twist_msg.twist.linear.y = vy;
                twist_msg.twist.linear.z = vz;
                twist_msg.twist.angular.x = wx;
                twist_msg.twist.angular.y = wy;
                twist_msg.twist.angular.z = wz;
                twist_pub_.publish(twist_msg);
            }
        }
        
        // 广播TF变换
        geometry_msgs::TransformStamped transform_stamped;
        transform_stamped.header.stamp = timestamp;
        transform_stamped.header.frame_id = map_frame_id_;
        transform_stamped.child_frame_id = base_frame_id_;
        transform_stamped.transform.translation.x = current_pose_.getOrigin().x();
        transform_stamped.transform.translation.y = current_pose_.getOrigin().y();
        transform_stamped.transform.translation.z = current_pose_.getOrigin().z();
        transform_stamped.transform.rotation.x = current_pose_.getRotation().x();
        transform_stamped.transform.rotation.y = current_pose_.getRotation().y();
        transform_stamped.transform.rotation.z = current_pose_.getRotation().z();
        transform_stamped.transform.rotation.w = current_pose_.getRotation().w();
        tf_broadcaster_.sendTransform(transform_stamped);
    }
};

int main(int argc, char** argv)
{
    ros::init(argc, argv, "ndt_localizer_node");
    NDTLocalizer localizer;
    ros::spin();
    return 0;
} 